%% no measurements available
%% [EKF can only forward simulate and keep track of covariance]

clear; clc; close all;
%load Q1_example_data.mat
load Q1_data.mat

% Loading gives us:
% Q - Model Noise
% Sigma0 - initial uncertainty
% dt - step size
% u - vector of measurements
% x0 - initial x position

T = length(u);
ss = length(x0);
xfilt = x0;
Sigma_filt{1} = Sigma0;


% Preallocate for speed
Sigma_filt = cell(T,1);
Sigma_filt{1} = Sigma0;

for t=1:T-1


    xfilt(:,t+1) = f_robot(xfilt(:,t), u(:,t), dt); % your code goes into f_robot.m
    
    A = jacobian_f_robot(xfilt(:,t), u(:,t), dt); % your code goes into jacobian_f_robot.m
    
    Sigma_filt{t+1} = A*Sigma_filt{t}*A' + Q;% your code here

% 	if(t==2)
% 		zA = A;
% 		zx = xfilt(:,t);
% 		zsf = Sigma_filt{t};
% 	end
	
end

%return


% plot subsampled trajectory:
colors1 = ['km']; 
map_fig_id = figure; hold on; axis([-5 25 -5 25]); axis equal; xlabel('East'); ylabel('North');
spacing = 20;
for i= [1:spacing:size(xfilt,2) size(xfilt,2)]
    plot_uncertainty_ellipse(xfilt(1:3,i), Sigma_filt{i}, map_fig_id, colors1);
end











